// 2025/4/7: 首个版本
// 2025/6/12: 协议名常量改为public

package com.appox.samples;

import com.appox.Sample;
import com.appox.GeneralSample;
import com.appox.GeneralElement;
import java.awt.Color;
import java.util.ArrayList;
import java.util.List;

/**
 * 全局目标物样本数据类
 */
public class GlobalObjectsSample extends Sample {

    /**
     * 动态目标物类型
     */
    public enum DynamicObjectType {
        UNKNOWN(0),
        CAR(1),       // 轿车
        VAN(2),       // 面包车
        TRUCK(3),     // 货车
        TRAILER(4),   // 拖车
        SEMITRAILER(5), // 货柜车
        BUS(6),       // 大巴
        MOTORBIKE(7), // 摩托车
        BICYCLE(8),   // 自行车
        TRAIN(9),     // 火车
        TRAM(10),     // 有轨电车
        PEDESTRIAN(11), // 行人
        WHEELCHAIR(12), // 轮椅
        ANIMAL(13);   // 动物
        
        private final int value;
        
        DynamicObjectType(int value) {
            this.value = value;
        }
        
        public int getValue() {
            return value;
        }
        
        public static DynamicObjectType fromValue(int value) {
            for (DynamicObjectType type : DynamicObjectType.values()) {
                if (type.getValue() == value) {
                    return type;
                }
            }
            return UNKNOWN;
        }
    }
    
    /**
     * 动态目标物类
     */
    public static class DynamicObject {
        private int id; // 目标物ID，0表示为主目标物，1~表示一般目标物
        private DynamicObjectType type; // 目标物类型
        private double centerLongitude; // [°E] 目标物中心的经度
        private double centerLatitude; // [°N] 目标物中心的纬度，北为正方向
        private Double centerAltitude; // [m] 目标物中心的海拔
        private Double orientation; // [deg] 目标物朝向，北为0，逆时针为正方向
        private Double width; // [m] 目标物宽度
        private Double length; // [m] 目标物长度
        private Double height; // [m] 目标物高度
        private Double speed; // [KPH] 目标物速度
        private Double accelX; // [m/s²] 目标物纵向加速度
        private Double curvature; // [1/m] 目标物转弯曲率
        private Double yawRate; // [deg/s] 目标物横摆角速度
        private Double pitch; // [deg] 目标物俯仰角，车头朝下为正
        private Double roll; // [deg] 目标物横滚角，右侧朝下为正
        private Double rearAxisLongitudinalOffset; // [m] 目标物自身坐标系下，后轴中心与物体中心的纵向距离，恒为正
        private Double rearAxisHeightOffset; // [m] 目标物自身坐标系下，后轴中心与物体中心的高度差，恒为正
        private Color color; // 目标物颜色
        
        public DynamicObject() {
            this.type = DynamicObjectType.UNKNOWN;
        }
        
        public int getId() {
            return id;
        }
        
        public void setId(int id) {
            this.id = id;
        }
        
        public DynamicObjectType getType() {
            return type;
        }
        
        public void setType(DynamicObjectType type) {
            this.type = type;
        }
        
        public double getCenterLongitude() {
            return centerLongitude;
        }
        
        public void setCenterLongitude(double centerLongitude) {
            this.centerLongitude = centerLongitude;
        }
        
        public double getCenterLatitude() {
            return centerLatitude;
        }
        
        public void setCenterLatitude(double centerLatitude) {
            this.centerLatitude = centerLatitude;
        }
        
        public Double getCenterAltitude() {
            return centerAltitude;
        }
        
        public void setCenterAltitude(Double centerAltitude) {
            this.centerAltitude = centerAltitude;
        }
        
        public Double getOrientation() {
            return orientation;
        }
        
        public void setOrientation(Double orientation) {
            this.orientation = orientation;
        }
        
        public Double getWidth() {
            return width;
        }
        
        public void setWidth(Double width) {
            this.width = width;
        }
        
        public Double getLength() {
            return length;
        }
        
        public void setLength(Double length) {
            this.length = length;
        }
        
        public Double getHeight() {
            return height;
        }
        
        public void setHeight(Double height) {
            this.height = height;
        }
        
        public Double getSpeed() {
            return speed;
        }
        
        public void setSpeed(Double speed) {
            this.speed = speed;
        }
        
        public Double getAccelX() {
            return accelX;
        }
        
        public void setAccelX(Double accelX) {
            this.accelX = accelX;
        }
        
        public Double getCurvature() {
            return curvature;
        }
        
        public void setCurvature(Double curvature) {
            this.curvature = curvature;
        }
        
        public Double getYawRate() {
            return yawRate;
        }
        
        public void setYawRate(Double yawRate) {
            this.yawRate = yawRate;
        }
        
        public Double getPitch() {
            return pitch;
        }
        
        public void setPitch(Double pitch) {
            this.pitch = pitch;
        }
        
        public Double getRoll() {
            return roll;
        }
        
        public void setRoll(Double roll) {
            this.roll = roll;
        }
        
        public Double getRearAxisLongitudinalOffset() {
            return rearAxisLongitudinalOffset;
        }
        
        public void setRearAxisLongitudinalOffset(Double rearAxisLongitudinalOffset) {
            this.rearAxisLongitudinalOffset = rearAxisLongitudinalOffset;
        }
        
        public Double getRearAxisHeightOffset() {
            return rearAxisHeightOffset;
        }
        
        public void setRearAxisHeightOffset(Double rearAxisHeightOffset) {
            this.rearAxisHeightOffset = rearAxisHeightOffset;
        }
        
        public Color getColor() {
            return color;
        }
        
        public void setColor(Color color) {
            this.color = color;
        }
    }
    
    private List<DynamicObject> objects; // 目标物列表
    
    /**
     * 构造函数，初始化目标物列表
     */
    public GlobalObjectsSample() {
        objects = new ArrayList<>();
    }
    
    /**
     * 获取目标物列表
     */
    public List<DynamicObject> getObjects() {
        return objects;
    }
    
    /**
     * 设置目标物列表
     */
    public void setObjects(List<DynamicObject> objects) {
        this.objects = objects;
    }
    
    /**
     * 将全局目标物样本转换为通用样本
     */
    public GeneralSample toGeneralSample() {
        GeneralSample sample = new GeneralSample();
        sample.setTime(this);
        sample.setProtocol(TARGET_PROTOCOL);
        
        GeneralElement[] data = new GeneralElement[1 + 26 * objects.size()];
        
        // 设置目标物数量
        data[0] = new GeneralElement((double)objects.size());
        
        // 初始化所有元素
        for (int i = 1; i < data.length; i++) {
            data[i] = new GeneralElement();
        }
        
        int baseIndex = 1;
        for (int i = 0; i < objects.size(); i++) {
            DynamicObject obj = objects.get(i);
            data[baseIndex + 0] = new GeneralElement((double)obj.getId());
            data[baseIndex + 1] = new GeneralElement((double)obj.getType().getValue());
            data[baseIndex + 2] = new GeneralElement(obj.getCenterLongitude());
            data[baseIndex + 3] = new GeneralElement(obj.getCenterLatitude());
            if (obj.getCenterAltitude() != null) data[baseIndex + 4] = new GeneralElement(obj.getCenterAltitude());
            if (obj.getOrientation() != null) data[baseIndex + 5] = new GeneralElement(obj.getOrientation());
            if (obj.getWidth() != null) data[baseIndex + 6] = new GeneralElement(obj.getWidth());
            if (obj.getLength() != null) data[baseIndex + 7] = new GeneralElement(obj.getLength());
            if (obj.getHeight() != null) data[baseIndex + 8] = new GeneralElement(obj.getHeight());
            if (obj.getSpeed() != null) data[baseIndex + 9] = new GeneralElement(obj.getSpeed());
            if (obj.getAccelX() != null) data[baseIndex + 10] = new GeneralElement(obj.getAccelX());
            if (obj.getCurvature() != null) data[baseIndex + 11] = new GeneralElement(obj.getCurvature());
            if (obj.getYawRate() != null) data[baseIndex + 12] = new GeneralElement(obj.getYawRate());
            if (obj.getPitch() != null) data[baseIndex + 13] = new GeneralElement(obj.getPitch());
            if (obj.getRoll() != null) data[baseIndex + 14] = new GeneralElement(obj.getRoll());
            if (obj.getRearAxisLongitudinalOffset() != null) data[baseIndex + 15] = new GeneralElement(obj.getRearAxisLongitudinalOffset());
            if (obj.getRearAxisHeightOffset() != null) data[baseIndex + 16] = new GeneralElement(obj.getRearAxisHeightOffset());
            if (obj.getColor() != null) {
                data[baseIndex + 17] = new GeneralElement((double)obj.getColor().getRed());
                data[baseIndex + 18] = new GeneralElement((double)obj.getColor().getGreen());
                data[baseIndex + 19] = new GeneralElement((double)obj.getColor().getBlue());
            }
            
            baseIndex += 26;
        }
        
        sample.setValues(data);
        sample.setNumberOfSignificants(data.length);
        
        return sample;
    }
    
    /**
     * 获取通用样本协议
     */
    @Override
    public String getGeneralSampleProtocol() {
        return TARGET_PROTOCOL;
    }
    
    /**
     * 获取通用样本协议列表
     */
    @Override
    public String[] getGeneralSampleProtocols() {
        return PROTOCOLS;
    }
    
    /**
     * 从通用样本转换为全局目标物样本
     */
    @Override
    public boolean fromGeneralSample(GeneralSample sample) {
        setTime(sample);
        
        String protocol = sample.getProtocol();
        
        // 根据协议版本调用相应的处理函数
        if ("global-objects-sample-v2".equals(protocol)) {
            return fromGlobalObjectsSampleV2(sample);
        } else if ("global-objects-sample-v1".equals(protocol)) {
            return fromGlobalObjectsSampleV1(sample);
        }
        
        return false;
    }
    
    /**
     * 从通用样本V2版本转换为全局目标物样本
     */
    private boolean fromGlobalObjectsSampleV2(GeneralSample sample) {
        GeneralElement[] values = sample.getValues();
        if (values == null || values.length < 1 || !values[0].isValid()) {
            return false;
        }
        
        int objectCount = (int)values[0].getValue();
        if (values.length != 1 + 26 * objectCount) {
            return false;
        }
        
        objects = new ArrayList<>();
        int baseIndex = 1;
        for (int i = 0; i < objectCount; i++) {
            DynamicObject obj = new DynamicObject();
            obj.setId((int)values[baseIndex + 0].getValue());
            obj.setType(DynamicObjectType.fromValue((int)values[baseIndex + 1].getValue()));
            obj.setCenterLongitude(values[baseIndex + 2].getValue());
            obj.setCenterLatitude(values[baseIndex + 3].getValue());
            if (values[baseIndex + 4].isValid()) obj.setCenterAltitude(values[baseIndex + 4].getValue());
            if (values[baseIndex + 5].isValid()) obj.setOrientation(values[baseIndex + 5].getValue());
            if (values[baseIndex + 6].isValid()) obj.setWidth(values[baseIndex + 6].getValue());
            if (values[baseIndex + 7].isValid()) obj.setLength(values[baseIndex + 7].getValue());
            if (values[baseIndex + 8].isValid()) obj.setHeight(values[baseIndex + 8].getValue());
            if (values[baseIndex + 9].isValid()) obj.setSpeed(values[baseIndex + 9].getValue());
            if (values[baseIndex + 10].isValid()) obj.setAccelX(values[baseIndex + 10].getValue());
            if (values[baseIndex + 11].isValid()) obj.setCurvature(values[baseIndex + 11].getValue());
            if (values[baseIndex + 12].isValid()) obj.setYawRate(values[baseIndex + 12].getValue());
            if (values[baseIndex + 13].isValid()) obj.setPitch(values[baseIndex + 13].getValue());
            if (values[baseIndex + 14].isValid()) obj.setRoll(values[baseIndex + 14].getValue());
            if (values[baseIndex + 15].isValid()) obj.setRearAxisLongitudinalOffset(values[baseIndex + 15].getValue());
            if (values[baseIndex + 16].isValid()) obj.setRearAxisHeightOffset(values[baseIndex + 16].getValue());
            
            if (values[baseIndex + 17].isValid() && values[baseIndex + 18].isValid() && values[baseIndex + 19].isValid()) {
                int r = (int)values[baseIndex + 17].getValue();
                int g = (int)values[baseIndex + 18].getValue();
                int b = (int)values[baseIndex + 19].getValue();
                obj.setColor(new Color(r, g, b));
            }
            
            objects.add(obj);
            baseIndex += 26;
        }
        
        return true;
    }
    
    /**
     * 从通用样本V1版本转换为全局目标物样本
     */
    private boolean fromGlobalObjectsSampleV1(GeneralSample sample) {
        GeneralElement[] values = sample.getValues();
        if (values == null || values.length < 1 || !values[0].isValid()) {
            return false;
        }
        
        int objectCount = (int)values[0].getValue();
        if (values.length != 1 + 10 * objectCount) {
            return false;
        }
        
        objects = new ArrayList<>();
        int baseIndex = 1;
        for (int i = 0; i < objectCount; i++) {
            DynamicObject obj = new DynamicObject();
            obj.setId((int)values[baseIndex + 0].getValue());
            obj.setType(DynamicObjectType.fromValue((int)values[baseIndex + 1].getValue()));
            obj.setCenterLongitude(values[baseIndex + 2].getValue());
            obj.setCenterLatitude(values[baseIndex + 3].getValue());
            if (values[baseIndex + 4].isValid()) obj.setCenterAltitude(values[baseIndex + 4].getValue());
            if (values[baseIndex + 5].isValid()) obj.setOrientation(values[baseIndex + 5].getValue());
            if (values[baseIndex + 6].isValid()) obj.setWidth(values[baseIndex + 6].getValue());
            if (values[baseIndex + 7].isValid()) obj.setLength(values[baseIndex + 7].getValue());
            if (values[baseIndex + 8].isValid()) obj.setHeight(values[baseIndex + 8].getValue());
            if (values[baseIndex + 9].isValid()) obj.setSpeed(values[baseIndex + 9].getValue());
            
            objects.add(obj);
            baseIndex += 10;
        }
        
        return true;
    }
    
    /**
     * 是否支持插值
     */
    @Override
    public boolean supportInterpolation() {
        return true;
    }
    
    /**
     * 插值生成中间样本
     */
    @Override
    public Sample interpolate(SearchResult result) {
        GlobalObjectsSample s1 = (GlobalObjectsSample) result.getS1();
        GlobalObjectsSample s2 = (GlobalObjectsSample) result.getS2();
        double w1 = result.getW1();
        double w2 = result.getW2();
        
        GlobalObjectsSample output = new GlobalObjectsSample();
        
        for (DynamicObject s1Obj : s1.getObjects()) {
            for (DynamicObject s2Obj : s2.getObjects()) {
                if (s1Obj.getId() == s2Obj.getId()) {
                    DynamicObject obj = new DynamicObject();
                    obj.setId(s1Obj.getId());
                    obj.setType(w1 > w2 ? s1Obj.getType() : s2Obj.getType());
                    obj.setCenterLongitude(s1Obj.getCenterLongitude() * w1 + s2Obj.getCenterLongitude() * w2);
                    obj.setCenterLatitude(s1Obj.getCenterLatitude() * w1 + s2Obj.getCenterLatitude() * w2);
                    obj.setCenterAltitude(interpolateValue(s1Obj.getCenterAltitude(), s2Obj.getCenterAltitude(), w1, w2));
                    obj.setOrientation(interpolateAngle(s1Obj.getOrientation(), s2Obj.getOrientation(), w1, w2));
                    obj.setWidth(interpolateValue(s1Obj.getWidth(), s2Obj.getWidth(), w1, w2));
                    obj.setLength(interpolateValue(s1Obj.getLength(), s2Obj.getLength(), w1, w2));
                    obj.setHeight(interpolateValue(s1Obj.getHeight(), s2Obj.getHeight(), w1, w2));
                    obj.setSpeed(interpolateValue(s1Obj.getSpeed(), s2Obj.getSpeed(), w1, w2));
                    obj.setAccelX(interpolateValue(s1Obj.getAccelX(), s2Obj.getAccelX(), w1, w2));
                    obj.setCurvature(interpolateValue(s1Obj.getCurvature(), s2Obj.getCurvature(), w1, w2));
                    obj.setYawRate(interpolateValue(s1Obj.getYawRate(), s2Obj.getYawRate(), w1, w2));
                    obj.setPitch(interpolateValue(s1Obj.getPitch(), s2Obj.getPitch(), w1, w2));
                    obj.setRoll(interpolateValue(s1Obj.getRoll(), s2Obj.getRoll(), w1, w2));
                    obj.setRearAxisLongitudinalOffset(interpolateValue(s1Obj.getRearAxisLongitudinalOffset(), s2Obj.getRearAxisLongitudinalOffset(), w1, w2));
                    obj.setRearAxisHeightOffset(interpolateValue(s1Obj.getRearAxisHeightOffset(), s2Obj.getRearAxisHeightOffset(), w1, w2));
                    
                    // 对于颜色，使用权重较大一方的颜色
                    obj.setColor(w1 > w2 ? s1Obj.getColor() : s2Obj.getColor());
                    
                    output.getObjects().add(obj);
                    break;
                }
            }
        }
        
        return output;
    }
    
    /**
     * 对数值进行插值，处理null情况
     */
    private Double interpolateValue(Double v1, Double v2, double w1, double w2) {
        if (v1 == null && v2 == null) return null;
        if (v1 == null) return v2;
        if (v2 == null) return v1;
        return v1 * w1 + v2 * w2;
    }
    
    /**
     * 对角度进行插值，使用正弦和余弦计算
     */
    private Double interpolateAngle(Double a1, Double a2, double w1, double w2) {
        if (a1 == null || a2 == null) return null;
        
        double deg2rad = Math.PI / 180;
        double x1 = Math.cos(a1 * deg2rad);
        double y1 = Math.sin(a1 * deg2rad);
        double x2 = Math.cos(a2 * deg2rad);
        double y2 = Math.sin(a2 * deg2rad);
        
        double xo = x1 * w1 + x2 * w2;
        double yo = y1 * w1 + y2 * w2;
        if (xo == 0 && yo == 0) return null;
        
        return Math.atan2(yo, xo) / deg2rad;
    }
    
    // 协议相关常量
    public static final String TARGET_PROTOCOL = "global-objects-sample-v2";
    public static final String[] PROTOCOLS = {
        "global-objects-sample-v1", "global-objects-sample-v2"
    };
} 